//
// Created by daybeha on 23-11-21.
//

#include "image_node.h"

image_node::image_node(): Node("image_node"){
    pub_l = image_transport::create_publisher(this, "/kitti/left/image_raw");
    pub_r = image_transport::create_publisher(this,"/kitti/right/image_raw");
    sub_l = image_transport::create_subscription(this, "/kitti/camera_gray_left/image_raw", bind(&image_node::imageLCallback, this, std::placeholders::_1), "raw");
    sub_r = image_transport::create_subscription(this, "/kitti/camera_gray_right/image_raw", bind(&image_node::imageRCallback, this, std::placeholders::_1), "raw");

    format = "mono8";

}


void image_node::imageLCallback(const sensor_msgs::msg::Image::ConstSharedPtr &msg) {
    try {
        cv::Mat img = cv_bridge::toCvShare(msg, format)->image;
        cv::imshow("view_l", img);
        cv::waitKey(1);

        pub_l.publish(msg);

    } catch (const cv_bridge::Exception &e) {
        auto logger = rclcpp::get_logger("my_subscriber");
        RCLCPP_ERROR(logger, "Could not convert from '%s' to '%s'" , msg->encoding.c_str(), format.c_str());
    }
}

void image_node::imageRCallback(const sensor_msgs::msg::Image::ConstSharedPtr &msg) {
    try {
        cv::Mat img = cv_bridge::toCvShare(msg, format)->image;
        cv::imshow("view_r", img);
        cv::waitKey(1);
//        cout << img.channels() << endl;

        pub_r.publish(msg);

    } catch (const cv_bridge::Exception &e) {
        auto logger = rclcpp::get_logger("my_subscriber");
        RCLCPP_ERROR(logger, "Could not convert from '%s' to '%s'" , msg->encoding.c_str(), format.c_str());
    }
}
